#include "zbControl.h"

// TO STOP WARNINGS ! 
void USART_Configuration(void);
void CameraStartup(void);
void DCMI_TakeSnapShot(void);
void initEnviromental(void);

// global control vars 
struct glbVars *mygVars;
u8 g_Vars[g_VarsSize]; // 100 bytes for our "global" vars
pgloabalVars newgVars;
extern u8 camState; 


void doTasks(void)
{
	// update pwm 
	updatePWMOUT();
	// send or rx new i2c data 
	//updateI2CMain();
	// adc 
	updateADC();
	// gpio motors, lights etc
	updateGPIO();
	// todo 
	//updateCAM();
}



void startupSystems(void)
{
	initEnviromental();
	initADC();
    // debug usart config
	USART_Configuration();
	printf("\r\n\r\nWelcome to ZBot debug output\r\n");
	// todo init vars function
	// start up the cam
	initCAM();

	// NOTE THESE HAD TO BE IN THIS ORDER WHEN INIT CAM WAS
	// AT THE END USART STOPPED WORKING ... IDK BUT BE CAREFULL
	// 
	initTime(); 
	//-initSystems();
	initProtocol();
	//-initLed();
	initComm();
	
	initGPIO();
	initPWMOUT(); // stop motors !

}

/**
  * @brief  init the body type and version of software, future will be from gpio?
  * these will be static specific things that can let our polling engine know if 
  * we have things like video or other things that it may try to use 
  * @param  None
  * @retval None
  */
void initEnviromental()
{
	// todo remove byte array, use struct ? 
	// point at our byte array
	mygVars = (void*)&g_Vars; // this is what gets sent to client

	// todo get from gpio
	// software verson 01, model ver 01 (tracked)
	mygVars->modelInfo = 0x01;
	mygVars->versionInfo = 0x01;

	mygVars->VideoWidth = frame_width;
    mygVars->VideoHeight = frame_height;
	mygVars->VideoBPP = 2; // two bytes per pixel
	mygVars->VideoFmt = 0; // rgb ? no clear defines yet
	mygVars->VideoStat = 0;


	mygVars->dataState= 1; // start in idle state
	newgVars->PWMOUTFQ = ZPWM0_FQ;
	mygVars->PWMOUTFQ = ZPWM0_FQ;

	takeNextSnapShot(); // will fill framebufferA with first image



}
